# -*- coding: utf-8 -*-

from threading import Lock

import rospy
from tecbot_msgs.msg import flow_state
from tecbot_msgs.srv import (flow_action, flow_actionRequest, flow_actionResponse,
                             flow_cmd, flow_cmdRequest, flow_cmdResponse)

import logger
import symbols
import utils

# const part
interactive_back = symbols.cmd_code['back']
interactive_pause = symbols.cmd_code['pause']
interactive_continue = symbols.cmd_code['continue']
interactive_wait = symbols.cmd_code['wait']

# action control part
flow_action_updated = False
flow_action_action = symbols.flow_action_code['continue']
flow_action_arg = None

# state part
state_lock = Lock()
flow_state_data = flow_state()

# mutex part
lock = Lock()

# ros proxy init
flow_initialized = False


def init_flow():
    global flow_state_pub
    global flow_initialized

    logger.loginfo(handler='init_flow',
                   text='正在启动 ROS 代理...')

    ret = symbols.cmd_code['ok']

    if not flow_initialized:
        def flow_action_service_callback(req):
            global flow_action_updated
            global flow_action_action
            global flow_action_arg
            global lock

            assert isinstance(req, flow_actionRequest)

            lock.acquire()

            flow_action_updated = True
            flow_action_action = symbols.flow_action_code[req.action]
            flow_action_arg = req.arg

            lock.release()

            return flow_actionResponse(symbols.cmd_code['ok'])

        def send_flow_state(_):
            global flow_state_data

            state_lock.acquire()

            flow_state_data.header.frame_id = 'map'
            flow_state_data.header.stamp = utils.current_ros_time()

            flow_state_pub.publish(flow_state_data)

            state_lock.release()

        try:
            _ = rospy.Service(
                'tecbot/flow/action', flow_action, flow_action_service_callback)

            flow_state_pub = rospy.Publisher("tecbot/flow/state", flow_state,
                                             queue_size=10, latch=True)

            set_flow_state(state=symbols.flow_state_code['starting'])

            rospy.Timer(rospy.Duration(0.2), send_flow_state)

            flow_initialized = True

            ret = symbols.cmd_code['ok']
        except:
            ret = symbols.cmd_code['error']

    if ret == symbols.cmd_code['ok']:
        logger.logdebug(
            handler='init_flow', text='函数执行结果为 [%s]。' % str(ret))
    else:
        logger.logerror(
            handler='init_flow', text='函数执行结果为 [%s]。' % str(ret))

    return ret

# setters


def set_flow_state(state=None, x=None, y=None, yaw=None, voltage=None, percent=None):
    global flow_state_data

    state_lock.acquire()

    if (state != None):
        flow_state_data.state = symbols.flow_state_code[state]

    if (x != None):
        flow_state_data.pos.x = x

    if (y != None):
        flow_state_data.pos.y = y

    if (yaw != None):
        flow_state_data.pos.yaw = yaw
    
    if (voltage != None):
        flow_state_data.voltage = voltage

    if (percent != None):
        flow_state_data.percent = percent

    state_lock.release()

# senders


def send_flow_cmd(cmd, arg):
    rospy.wait_for_service(
        'tecbot/flow/cmd', timeout=10)

    flow_cmd_service = rospy.ServiceProxy(
        'tecbot/flow/cmd', flow_cmd)

    received_data = flow_cmd_service.call(
        flow_cmdRequest(cmd, arg))
    assert isinstance(received_data, flow_cmdResponse)

    return received_data.result

# receivers


def receive_flow_action(latch=False):
    global flow_action_updated

    lock.acquire()

    action = flow_action_action

    if flow_action_updated:
        flow_action_updated = False

        lock.release()

        return symbols.cmd_code[action]
    elif latch:
        lock.release()

        return symbols.cmd_code[action]
    else:
        lock.release()

        return symbols.cmd_code['wait']

# controllers


def if_to_back(cmd):
    return cmd == interactive_back


def if_to_pause(cmd):
    return cmd == interactive_pause


def if_to_continue(cmd):
    return cmd == interactive_continue
